#include "mount_siyi.h"
#include <thread>

void siyiUpdateHandler();
void siyiRtspHandler();
static void siyiVideoCallbackHandler(cv::Mat image);
static void siyiAttitudeCallbackHandler(float roll, float pitch, float yaw);


int main()
{
	printf("mount siyi test\n");

	//start siyi update
	std::thread siyiUpdate(siyiUpdateHandler);

	//start siyi rtsp
	std::thread siyiRtsp(siyiRtspHandler);

	mountSiyiHandler.registerSiyiVideoCallback(siyiVideoCallbackHandler);

	mountSiyiHandler.registerSiyiAttitudeCallback(siyiAttitudeCallbackHandler);

	usleep(1000 * 1000);
	mountSiyiHandler.center_gimbal();

	while(1)
	{
		// 1s
		usleep(1000 * 1000);
	}

	siyiUpdate.join();
	siyiRtsp.join();
	return 0;
}


void siyiUpdateHandler()
{
	bool ret = mountSiyiHandler.init("udp", "192.168.10.128");
	if(!ret)
	{
		printf("siyi init error\n");
		return;
	}
	mountSiyiHandler.update();
}


void siyiRtspHandler()
{
	bool ret = mountSiyiHandler.startRtspRecv("rtsp://192.168.10.128:8554/main.264");
	if(!ret)
	{
		printf("start rtsp recv error\n");
		return;
	}
}


static void siyiVideoCallbackHandler(cv::Mat image)
{

	cv::Mat im;
	im = image.clone();
	printf("siyi video callback handler!\n");
}


static void siyiAttitudeCallbackHandler(float roll, float pitch, float yaw)
{
	printf("siyi gimbal roll:%f, pitch:%f ,yaw:%f\n", roll, pitch, yaw);
}

